#include "fpal.hpp" // 包含头文件，提供所有类型定义和函数声明

// -----------------------------------------------------------
// 封装在命名空间 fpal 中
// -----------------------------------------------------------
namespace fpal {

// --- 复杂运算集合 - 实现 (非模板函数) ---

// 平方根函数 (fp_coord_t 版本)
fp_coord_t sqrt(fp_coord_t val) {
    #pragma HLS PIPELINE II=1
    return custom_sqrt_iterative<fp_coord_t, 3>(val);
}
// 平方根函数 (fp_vec_unit_t 版本)
fp_vec_unit_t sqrt(fp_vec_unit_t val) {
    #pragma HLS PIPELINE II=1
    return custom_sqrt_iterative<fp_vec_unit_t, 3>(val);
}

// 倒数平方根函数 (fp_coord_t 版本)
fp_coord_t inv_sqrt(fp_coord_t val) {
    #pragma HLS PIPELINE II=1
    return custom_inv_sqrt_iterative<fp_coord_t, 3>(val);
}
// 倒数平方根函数 (fp_vec_unit_t 版本)
fp_vec_unit_t inv_sqrt(fp_vec_unit_t val) {
    #pragma HLS PIPELINE II=1
    return custom_inv_sqrt_iterative<fp_vec_unit_t, 3>(val);
}

// -----------------------------------------------------------
// 3. 线性代数运算集合 - 实现
// -----------------------------------------------------------

// 向量加法 (vec3_fp_coord 类型)
vec3_fp_coord vec3_add(vec3_fp_coord v1, vec3_fp_coord v2) {
    #pragma HLS PIPELINE II=1
    vec3_fp_coord res;
    res.x = add(v1.x, v2.x);
    res.y = add(v1.y, v2.y);
    res.z = add(v1.z, v2.z);
    return res;
}
// 向量加法 (vec3_fp_vec_unit 类型)
vec3_fp_vec_unit vec3_add(vec3_fp_vec_unit v1, vec3_fp_vec_unit v2) {
    #pragma HLS PIPELINE II=1
    vec3_fp_vec_unit res;
    res.x = add(v1.x, v2.x);
    res.y = add(v1.y, v2.y);
    res.z = add(v1.z, v2.z);
    return res;
}

// 向量减法 (vec3_fp_coord 类型)
vec3_fp_coord vec3_sub(vec3_fp_coord v1, vec3_fp_coord v2) {
    #pragma HLS PIPELINE II=1
    vec3_fp_coord res;
    res.x = sub(v1.x, v2.x);
    res.y = sub(v1.y, v2.y);
    res.z = sub(v1.z, v2.z);
    return res;
}
// 向量减法 (vec3_fp_vec_unit 类型)
vec3_fp_vec_unit vec3_sub(vec3_fp_vec_unit v1, vec3_fp_vec_unit v2) {
    #pragma HLS PIPELINE II=1
    vec3_fp_vec_unit res;
    res.x = sub(v1.x, v2.x);
    res.y = sub(v1.y, v2.y);
    res.z = sub(v1.z, v2.z);
    return res;
}

// 点积 (vec3_fp_vec_unit 类型)
fp_vec_unit_t vec3_dot(vec3_fp_vec_unit v1, vec3_fp_vec_unit v2) {
    #pragma HLS PIPELINE II=1
    return add(add(mul(v1.x, v2.x), mul(v1.y, v2.y)), mul(v1.z, v2.z));
}
// 点积 (vec3_fp_coord 类型)
fp_coord_t vec3_dot(vec3_fp_coord v1, vec3_fp_coord v2) {
    #pragma HLS PIPELINE II=1
    return add(add(mul(v1.x, v2.x), mul(v1.y, v2.y)), mul(v1.z, v2.z));
}

// 叉积 (vec3_fp_vec_unit 类型)
vec3_fp_vec_unit vec3_cross(vec3_fp_vec_unit v1, vec3_fp_vec_unit v2) {
    #pragma HLS PIPELINE II=1
    vec3_fp_vec_unit res;
    res.x = sub(mul(v1.y, v2.z), mul(v1.z, v2.y));
    res.y = sub(mul(v1.z, v2.x), mul(v1.x, v2.z));
    res.z = sub(mul(v1.x, v2.y), mul(v1.y, v2.x));
    return res;
}
// 叉积 (vec3_fp_coord 类型)
vec3_fp_coord vec3_cross(vec3_fp_coord v1, vec3_fp_coord v2) {
    #pragma HLS PIPELINE II=1
    vec3_fp_coord res;
    res.x = sub(mul(v1.y, v2.z), mul(v1.z, v2.y));
    res.y = sub(mul(v1.z, v2.x), mul(v1.x, v2.z));
    res.z = sub(mul(v1.x, v2.y), mul(v1.y, v2.x));
    return res;
}

// 向量长度平方 (vec3_fp_vec_unit 类型)
fp_vec_unit_t vec3_length_sq(vec3_fp_vec_unit v) {
    #pragma HLS PIPELINE II=1
    return vec3_dot(v, v);
}
// 向量长度平方 (vec3_fp_coord 类型)
fp_coord_t vec3_length_sq(vec3_fp_coord v) {
    #pragma HLS PIPELINE II=1
    return vec3_dot(v, v);
}
// 向量长度平方 (fp_coord_t 类型，直接传入分量)
fp_coord_t vec3_length_sq(fp_coord_t x, fp_coord_t y, fp_coord_t z) {
    #pragma HLS PIPELINE II=1
    return add(add(mul(x,x), mul(y,y)), mul(z,z));
}
// 向量长度平方 (fp_vec_unit_t 类型，直接传入分量)
fp_vec_unit_t vec3_length_sq(fp_vec_unit_t x, fp_vec_unit_t y, fp_vec_unit_t z) {
    #pragma HLS PIPELINE II=1
    return add(add(mul(x,x), mul(y,y)), mul(z,z));
}


// 向量长度 (vec3_fp_vec_unit 类型)
fp_vec_unit_t vec3_length(vec3_fp_vec_unit v) {
    #pragma HLS PIPELINE II=1
    return sqrt(vec3_length_sq(v));
}
// 向量长度 (fp_coord_t 类型，直接传入分量)
fp_coord_t vec3_length(fp_coord_t x, fp_coord_t y, fp_coord_t z) {
    #pragma HLS PIPELINE II=1
    return sqrt(add(add(mul(x,x), mul(y,y)), mul(z,z)));
}
// 向量长度 (vec3_fp_coord 类型)
fp_coord_t vec3_length(vec3_fp_coord v) {
    #pragma HLS PIPELINE II=1
    return sqrt(vec3_length_sq(v));
}

// 向量归一化 (vec3_fp_vec_unit 类型)
vec3_fp_vec_unit vec3_normalize(vec3_fp_vec_unit v) {
    #pragma HLS PIPELINE II=1
    fp_vec_unit_t len_sq = vec3_length_sq(v);

    // 避免除以零，并提供一个合理的默认值（例如，沿着Z轴的单位向量）
    fp_vec_unit_t normalization_lsb;
    normalization_lsb = 0;             // 首先清零所有位
    normalization_lsb.range(0, 0) = 1; // 设置最低位为 1
    fp_vec_unit_t normalization_threshold = normalization_lsb * (fp_vec_unit_t)8;

    if (len_sq < normalization_threshold)
        return {(fp_vec_unit_t)0,(fp_vec_unit_t)0,(fp_vec_unit_t)1.0};

    fp_vec_unit_t inv_l = inv_sqrt(len_sq);
    vec3_fp_vec_unit res;
    res.x = mul(v.x, inv_l);
    res.y = mul(v.y, inv_l);
    res.z = mul(v.z, inv_l);
    return res;
}
// 向量归一化 (vec3_fp_coord 类型)
vec3_fp_coord vec3_normalize(vec3_fp_coord v) {
    #pragma HLS PIPELINE II=1
    fp_coord_t len_sq = vec3_length_sq(v);

    fp_coord_t normalization_lsb;
    normalization_lsb = 0;             // 首先清零所有位
    normalization_lsb.range(0, 0) = 1; // 设置最低位为 1
    fp_coord_t normalization_threshold = normalization_lsb * (fp_coord_t)8;

    if (len_sq < normalization_threshold)
        return {(fp_coord_t)0,(fp_coord_t)0,(fp_coord_t)1.0};

    fp_coord_t inv_l = inv_sqrt(len_sq);
    vec3_fp_coord res;
    res.x = mul(v.x, inv_l);
    res.y = mul(v.y, inv_l);
    res.z = mul(v.z, inv_l);
    return res;
}
// 向量归一化 (fp_coord_t 类型，直接传入分量)
vec3_fp_coord vec3_normalize(fp_coord_t x, fp_coord_t y, fp_coord_t z) {
    #pragma HLS PIPELINE II=1
    fp_coord_t len_sq = vec3_length_sq(x, y, z); // 调用直接接受分量的 length_sq

    // 解决HLS 214-113警告：将常量声明为 static const
    static const fp_coord_t normalization_lsb_c = [] {
        fp_coord_t l = 0;
        l.range(0, 0) = 1;
        return l;
    }();
    static const fp_coord_t normalization_threshold_c = normalization_lsb_c * (fp_coord_t)8;

    if (len_sq < normalization_threshold_c)
        return {(fp_coord_t)0,(fp_coord_t)0,(fp_coord_t)1.0};

    fp_coord_t inv_l = inv_sqrt(len_sq);
    vec3_fp_coord res;
    res.x = mul(x, inv_l);
    res.y = mul(y, inv_l);
    res.z = mul(z, inv_l);
    return res;
}
// 向量归一化 (fp_vec_unit_t 类型，直接传入分量)
vec3_fp_vec_unit vec3_normalize(fp_vec_unit_t x, fp_vec_unit_t y, fp_vec_unit_t z) {
    #pragma HLS PIPELINE II=1
    fp_vec_unit_t len_sq = vec3_length_sq(x, y, z); // 调用直接接受分量的 length_sq

    // 解决HLS 214-113警告：将常量声明为 static const
    static const fp_vec_unit_t normalization_lsb_vu = [] {
        fp_vec_unit_t l = 0;
        l.range(0, 0) = 1;
        return l;
    }();
    static const fp_vec_unit_t normalization_threshold_vu = normalization_lsb_vu * (fp_vec_unit_t)8;

    if (len_sq < normalization_threshold_vu)
        return {(fp_vec_unit_t)0,(fp_vec_unit_t)0,(fp_vec_unit_t)1.0};

    fp_vec_unit_t inv_l = inv_sqrt(len_sq);
    vec3_fp_vec_unit res;
    res.x = mul(x, inv_l);
    res.y = mul(y, inv_l);
    res.z = mul(z, inv_l);
    return res;
}


// 矩阵-向量乘法 (4x4 矩阵 * 4元素向量，用于齐次坐标变换)
vec3_fp_coord mat4_vec4_mul(const fp_coord_t mat[4][4], vec3_fp_coord vec_xyz, fp_coord_t w_in) {
    #pragma HLS ARRAY_PARTITION variable=mat complete dim=1 
    #pragma HLS ARRAY_PARTITION variable=mat complete dim=2
    
    vec3_fp_coord res;
    
    res.x = add(add(mul(mat[0][0], vec_xyz.x), mul(mat[0][1], vec_xyz.y)), 
                add(mul(mat[0][2], vec_xyz.z), mul(mat[0][3], w_in)));
    res.y = add(add(mul(mat[1][0], vec_xyz.x), mul(mat[1][1], vec_xyz.y)),
                add(mul(mat[1][2], vec_xyz.z), mul(mat[1][3], w_in)));
    res.z = add(add(mul(mat[2][0], vec_xyz.x), mul(mat[2][1], vec_xyz.y)),
                add(mul(mat[2][2], vec_xyz.z), mul(mat[2][3], w_in)));
    
    return res;
}

// 矩阵-向量乘法 (3x3 矩阵 * 3元素向量)
vec3_fp_vec_unit mat3_vec3_mul(const fp_vec_unit_t mat[3][3], vec3_fp_vec_unit vec) {
    #pragma HLS ARRAY_PARTITION variable=mat complete dim=1
    #pragma HLS ARRAY_PARTITION variable=mat complete dim=2

    vec3_fp_vec_unit res;
    res.x = add(add(mul(mat[0][0], vec.x), mul(mat[0][1], vec.y)), mul(mat[0][2], vec.z));
    res.y = add(add(mul(mat[1][0], vec.x), mul(mat[1][1], vec.y)), mul(mat[1][2], vec.z));
    res.z = add(add(mul(mat[2][0], vec.x), mul(mat[2][1], vec.y)), mul(mat[2][2], vec.z));
    return res;
}

// -----------------------------------------------------------
// 4. 定点数类型转换函数 - 实现
//    为这些简单转换函数添加 HLS INLINE pragma
// -----------------------------------------------------------

// 将 fp_coord_t 转换为 fp_vec_unit_t
fp_vec_unit_t conv_coord_to_vec_unit(fp_coord_t val) {
    return (fp_vec_unit_t)val;
}

// 将 fp_vec_unit_t 转换为 fp_coord_t
fp_coord_t conv_vec_unit_to_coord(fp_vec_unit_t val) {
    return (fp_coord_t)val;
}

// 将标准化定点数转换为 8 位颜色通道 (0-255)
// val 预计在 [0.0, 1.0] 范围内，会先进行 saturate
fp_color_channel_t conv_fp_to_color_channel(fp_vec_unit_t val) {
    // 先将值饱和到 [0.0, 1.0] 范围
    fp_vec_unit_t saturated_val = saturate(val);
    // 乘以 255.0 得到 [0.0, 255.0] 的值。
    fp_color_scale_t scaled_val = (fp_color_scale_t)saturated_val * (fp_color_scale_t)255.0; 
    // 再次钳制到 [0.0, 255.0]，以防中间计算或 `fp_color_scale_t` 转换时出现微小溢出或下溢。
    fp_color_scale_t clamped_scaled_val = clamp(scaled_val, (fp_color_scale_t)0.0, (fp_color_scale_t)255.0);
    // 最后四舍五入并转换为 uint8。
    return round_to_nearest_uint8(clamped_scaled_val);
}

// 将标准化定点数转换为 8 位颜色通道 (0-255)
// val 预计在 [0.0, 1.0] 范围内，会先进行 saturate
fp_color_channel_t conv_fp_to_color_channel(fp_coord_t val) {
    // 先将值饱和到 [0.0, 1.0] 范围
    fp_coord_t saturated_val = clamp(val, (fp_coord_t)0.0, (fp_coord_t)1.0);
    // 乘以 255.0 得到 [0.0, 255.0] 的值。
    fp_coord_t scaled_val = saturated_val * (fp_coord_t)255.0;
    // 再次钳制到 [0.0, 255.0]。
    fp_coord_t clamped_scaled_val = clamp(scaled_val, (fp_coord_t)0.0, (fp_coord_t)255.0);
    // 注意：`round_to_nearest_uint8` 需要一个 `fp_color_scale_t` 类型作为输入，
    // 因此这里需要一次显式转换。
    return round_to_nearest_uint8((fp_color_scale_t)clamped_scaled_val);
}

} // namespace fpal
